import pygame, sys,os
from pygame.locals import * 
from math import pi, sin, cos
import time
import serial

#angle:=237.000000



 
def input(): 
	events = pygame.event.get()
	for event in events: 
		if event.type == QUIT: 
			sys.exit(0) 
		else: 
			pass
			#print event 
 
class AngleMeter:
	def __init__(self):
		self.position = (200, 200)
		self.radius = 180
		self.fg_color = (255, 0, 0)
		self.bg_color = (255, 240, 240)
		self.font = pygame.font.Font(None, self.radius / 5)
	
	def draw(self, surface, angle, phase):		
		pygame.draw.circle(surface, self.bg_color, self.position, self.radius, 0)
		pygame.draw.circle(surface, self.fg_color, self.position, self.radius, 2)
		
		text = self.font.render('Angle', True, self.fg_color, self.bg_color)
		textRect = text.get_rect()
		textRect.centerx = self.position[0]
		textRect.centery = self.position[1] + self.radius * 0.4
		surface.blit(text, textRect)
		
		if angle is not None:
			meter_pos = (self.position[0] + cos(angle * pi / 180) * self.radius * 0.9, self.position[1] - sin(angle * pi / 180) * self.radius * 0.9)
			pygame.draw.aaline(surface, (0, 0, 255), self.position, meter_pos, 4)
		
		if phase is not None:
			meter_pos = (self.position[0] + cos(phase * pi / 180) * self.radius * 0.9, self.position[1] - sin(phase * pi / 180) * self.radius * 0.9)
			pygame.draw.aaline(surface, (0, 255, 255), self.position, meter_pos, 4)
		
		pygame.draw.circle(surface, self.fg_color, self.position, 5, 0)


class Communication:
	def __init__(self, port, speed):
		self.ser = serial.Serial(port, speed, timeout = 0)
		self.buf = ""
		self.variables = {}
	
	def readline(self):
		self.buf += self.ser.read(512)
		pos = self.buf.find("\n")
		if pos >= 0:
			line, self.buf = self.buf[:pos], self.buf[pos+1:]
			if line.find("\r") > 0:
				line = line[:-1]
			return line
		return ''
	
	def work(self):
		while True:
			line = self.readline()
			
			if not line:
				return
			try:
				print "line=", line
				pos = line.find("=")
				pos2 = line.find(",")
				variable = "a"
				value = line[pos+1:pos2]
				print line, " val=", value
				self.variables[variable] = 360-float(value)
			except:
				pass
	
	def get_angle(self):
		if self.variables.has_key("a"):
			return float(self.variables["a"])
		else:
			return None
	
	def __getitem__(self, index):
		if self.variables.has_key(index):
			return float(self.variables[index])
		else:
			return None
		

if __name__ == "__main__":
	
	
	pygame.init() 
	window = pygame.display.set_mode((640, 480)) 
	pygame.display.set_caption('Micromouse dashboard') 
	screen = pygame.display.get_surface() 

	comm = Communication("/dev/ttyUSB0", 115200)

	angle_meter = AngleMeter()
	while True:
		comm.work()	
		angle = comm['a']
		phase = comm['p']
		phase_index = comm['pi']

		#print "phase index:", phase_index, phase, angle
		input()
		
		screen.fill((255, 255, 255))
		angle_meter.draw(screen, angle, phase)
		pygame.display.flip()
		
		#time.sleep(0.025)

